/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once

#include <tf2/utils.h>

#include <autoware_utils/geometry/geometry.h>
#include <autoware_utils/math/normalization.h>

namespace autoware_utils
{
struct PoseDeviation
{
  double lateral;
  double longitudinal;
  double yaw;
};

inline PoseDeviation calcPoseDeviation(
  const geometry_msgs::Pose & base_pose, const geometry_msgs::Pose & target_pose)
{
  PoseDeviation deviation;

  // Lateral / Longitudinal
  {
    const auto & base_point = base_pose.position;
    const auto & target_point = target_pose.position;

    const auto yaw = tf2::getYaw(base_pose.orientation);
    const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0};

    const auto dx = target_point.x - base_point.x;
    const auto dy = target_point.y - base_point.y;
    const Eigen::Vector3d diff_vec{dx, dy, 0};

    const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec);

    deviation.lateral = cross_vec.z();
    deviation.longitudinal = base_unit_vec.dot(diff_vec);
  }

  // Yaw
  {
    const auto base_yaw = tf2::getYaw(base_pose.orientation);
    const auto target_yaw = tf2::getYaw(target_pose.orientation);
    deviation.yaw = normalizeRadian(target_yaw - base_yaw);
  }

  return deviation;
}
}  // namespace autoware_utils
